﻿using System;
using System.Drawing;
using System.Drawing.Imaging;
using OpenCvSharp;
using System.Collections.Generic;
using System.Windows.Forms;

namespace GeoSketch2.Core
{
    /// <summary>
    /// The enumeration of rectification techniques
    /// </summary>
    public enum RectificationTechnique
    {
        /// <summary>
        /// Image rectification using known points
        /// </summary>
        Points,

        /// <summary>
        /// Image rectification using vanishing lines
        /// </summary>
        Lines,

        /// <summary>
        /// Image rectification using a cross
        /// </summary>
        Cross,

        /// <summary>
        /// Image rectification using differentes known distances
        /// </summary>
        Distances,
    }

    /// <summary>
    /// The class for image rectification
    /// </summary>
    public class ImageRectification
    {
        #region Fields
        private Bitmap source_image;
        private RectificationCoefficients direct_coeffs;//Coefficients for the direct transformation (original -> rectified)
        private RectificationCoefficients inverse_coeffs;//Coefficients for the inverse transformation (rectified -> original)
        private RectificationTechnique rectification_technique;//Rectification technique
        private Transformations.AffineTransformation2D object_2_image_transformation;
        private int MaxSize;//taille de l'image de sortie
        private Size rectified_image_size; 
        
        private Point2D32S clic_cross;
        private double cross_radius;

        private Point2D64D[] image_coordinates;
        private Point2D64D[] object_coordinates;

        private Line2D64D[] HorizontalVerticalLines;
        private Line2D64D[] ConstraintsLines;
        double ConstraintLine1distance;
        double ConstraintLine2Distance;

        private Line2D64D[] DistancesLines;
        private Line2D64D HorizontalVerticalLine;//Tag = true if horizontal line
        #endregion

        #region Constructor

        /// <summary>
        /// The image rectification object, from known points
        /// </summary>
        /// <param name="SourceImage">The image to rectify</param>
        /// <param name="OutputMaxsize">The maximum size of the rectified image (pixels)</param>
        /// <param name="coord_img">The image coordinates of the known points</param>        
        /// <param name="coord_obj">The object coordinate of the known points</param>
        public ImageRectification(Bitmap SourceImage, int OutputMaxsize, Point2D64D[] coord_img, Point2D64D[] coord_obj)
        {
            source_image = SourceImage;
            rectification_technique = RectificationTechnique.Points;
            image_coordinates = (Point2D64D[])coord_img.Clone();
            object_coordinates = (Point2D64D[])coord_obj.Clone();
            MaxSize = OutputMaxsize;
        }

        /// <summary>
        /// The image rectification object, from a clic on a cross
        /// </summary>
        /// <param name="SourceImage">The image to rectify</param>
        /// <param name="OutputMaxsize">The maximum size of the rectified image (pixels)</param>
        /// <param name="Clic">The image coordinates of the clic point (pixels)</param>        
        /// <param name="CrossRadius">The radius of the real cross (mm)</param>
        public ImageRectification(Bitmap SourceImage, int OutputMaxsize, Point2D32S Clic, double CrossRadius)
        {
            source_image = SourceImage;
            rectification_technique = RectificationTechnique.Cross;
            MaxSize = OutputMaxsize;
            clic_cross = Clic;
            cross_radius=CrossRadius;
        }

        /// <summary>
        /// The image rectification object, from a clic on a cross
        /// </summary>
        /// <param name="SourceImage">The image to rectify</param>
        /// <param name="OutputMaxsize">The maximum size of the rectified image (pixels)</param>
        /// <param name="HorizontalVerticalLines">The horizontal and the vertical lines (2H then 2V)</param>        
        /// <param name="ConstraintsLines">The constraintes lines 2 lines whose real distance is known</param>
        /// <param name="ConstraintLine1distance">The distance of the first constraint line</param>        
        /// <param name="ConstraintLine2distance">The distance of the second constraint line</param>
        public ImageRectification(Bitmap SourceImage, int OutputMaxsize, Line2D64D[] HorizontalVerticalLines, Line2D64D[] ConstraintsLines, double ConstraintLine1distance,double ConstraintLine2distance)
        {
            source_image = SourceImage;
            rectification_technique = RectificationTechnique.Lines;
            MaxSize = OutputMaxsize;
            image_coordinates = new Point2D64D[4];
            object_coordinates = new Point2D64D[4];
            this.HorizontalVerticalLines = HorizontalVerticalLines;
            this.ConstraintsLines = ConstraintsLines;
            this.ConstraintLine1distance = ConstraintLine1distance;
            this.ConstraintLine2Distance = ConstraintLine2distance;
        }

        /// <summary>
        /// The image rectification object, from a clic on a cross
        /// </summary>
        /// <param name="SourceImage">The image to rectify</param>
        /// <param name="OutputMaxsize">The maximum size of the rectified image (pixels)</param>
        /// <param name="DistancesLines">The lines which length are known</param>
        /// <param name="HorizontalVerticalLine">The horizontal or vertical line to give the orientation</param>    
        public ImageRectification(Bitmap SourceImage, int OutputMaxsize, Line2D64D[] DistancesLines,Line2D64D HorizontalVerticalLine)
        {
            source_image = SourceImage;
            rectification_technique = RectificationTechnique.Distances;
            MaxSize = OutputMaxsize;
            
            this.DistancesLines = DistancesLines;
            this.HorizontalVerticalLine = HorizontalVerticalLine;
        }
        #endregion

        #region Methods
        #region Rectification coefficients computation
        #region From known object and image points
        //Computation of the rectification coefficient from the image to the object
        //The rectification formula is :
        //  X = (a1 * x + a2 * y + a3) / (c1 * x + c2 * y + 1)
        //  Y = (b1 * x + b2 * y + b3) / (c1 * x + c2 * y + 1)
        // With :
        //  X : X-coordinates of a point in the world coordinates system
        //  Y : Y-coordinates of a point in the world coordinates system
        // x : x-coordinates of a point in the image coordinates system
        //  y : y-coordinates of a point in the image coordinates system
        //  a1, a2, a3, b1, b2, b3, c1, c2 : the rectification coefficients
        private RectificationCoefficients ComputeDirectCoeffsFromPoints(Point2D64D[] coord_image, Point2D64D[] coord_objet)
        {
            RectificationCoefficients rectification_coefficients = new RectificationCoefficients();

            int points_count = PointsCount;

            CvMat mat_a = Cv.CreateMat(points_count * 2, 8, MatrixType.F64C1);
            CvMat mat_b = Cv.CreateMat(points_count * 2, 1, MatrixType.F64C1);
            CvMat mat_x = Cv.CreateMat(8, 1, MatrixType.F64C1);

            for (int l = 0; l < points_count; l++)
            {
                Cv.mSet(mat_a, 2 * l, 0, coord_image[l].X);
                Cv.mSet(mat_a, 2 * l, 1, coord_image[l].Y);
                Cv.mSet(mat_a, 2 * l, 2, 1);
                Cv.mSet(mat_a, 2 * l, 3, 0);
                Cv.mSet(mat_a, 2 * l, 4, 0);
                Cv.mSet(mat_a, 2 * l, 5, 0);
                Cv.mSet(mat_a, 2 * l, 6, -coord_image[l].X * coord_objet[l].X);
                Cv.mSet(mat_a, 2 * l, 7, -coord_image[l].Y * coord_objet[l].X);
                Cv.mSet(mat_b, 2 * l, 0, coord_objet[l].X);
                Cv.mSet(mat_a, 2 * l + 1, 3, coord_image[l].X);
                Cv.mSet(mat_a, 2 * l + 1, 4, coord_image[l].Y);
                Cv.mSet(mat_a, 2 * l + 1, 5, 1);
                Cv.mSet(mat_a, 2 * l + 1, 0, 0);
                Cv.mSet(mat_a, 2 * l + 1, 1, 0);
                Cv.mSet(mat_a, 2 * l + 1, 2, 0);
                Cv.mSet(mat_a, 2 * l + 1, 6, -coord_image[l].X * coord_objet[l].Y);
                Cv.mSet(mat_a, 2 * l + 1, 7, -coord_image[l].Y * coord_objet[l].Y);
                Cv.mSet(mat_b, 2 * l + 1, 0, coord_objet[l].Y);
            }
            if (points_count < 4)
            {
                throw new Exception("More or equal to 4 points needed");
            }
            else if (points_count >= 4)
            {
                Cv.Solve(mat_a, mat_b, mat_x, InvertMethod.Svd);
            }

            rectification_coefficients.a1 = Cv.mGet(mat_x, 0, 0);
            rectification_coefficients.a2 = Cv.mGet(mat_x, 1, 0);
            rectification_coefficients.a3 = Cv.mGet(mat_x, 2, 0);
            rectification_coefficients.b1 = Cv.mGet(mat_x, 3, 0);
            rectification_coefficients.b2 = Cv.mGet(mat_x, 4, 0);
            rectification_coefficients.b3 = Cv.mGet(mat_x, 5, 0);
            rectification_coefficients.c1 = Cv.mGet(mat_x, 6, 0);
            rectification_coefficients.c2 = Cv.mGet(mat_x, 7, 0);

            return rectification_coefficients;
        }

        //Computes the direct and inverse coefficients of the transformation
        //The indirect coefficients are used the generate the rectified image
        private void ComputeCoeffsFromPoints()
        {
            direct_coeffs = ComputeDirectCoeffsFromPoints(image_coordinates, object_coordinates);

            Point2D64D[] rectified_image_bounds = new Point2D64D[4];
            Point2D64D[] source_image_bounds = new RectangleD(0, 0, source_image.Width, source_image.Height).ToArray();

            for (int i = 0; i < 4; i++)
            {
                rectified_image_bounds[i] = ComputeObjectCoordinates(source_image_bounds[i]);
            }

            RectangleD rectified_image_bounds_R = Tools.GetRectangleDBounds(rectified_image_bounds,YAxisDirection.DownSide);
            RectangleD rectified_image_target_bounds = new RectangleD();

            rectified_image_target_bounds.Width = MaxSize;
            rectified_image_target_bounds.Height = rectified_image_target_bounds.Width * rectified_image_bounds_R.Height / rectified_image_bounds_R.Width;
            if (rectified_image_target_bounds.Height > MaxSize)
            {
                rectified_image_target_bounds.Height = MaxSize;
                rectified_image_target_bounds.Width = rectified_image_target_bounds.Height * rectified_image_bounds_R.Width / rectified_image_bounds_R.Height;
            }

            rectified_image_size = new Size((int)Math.Round(rectified_image_target_bounds.Width), (int)Math.Round(rectified_image_target_bounds.Height));

            object_2_image_transformation = new Transformations.AffineTransformation2D();
            object_2_image_transformation.ComputeTranformationFromTargetArea(rectified_image_bounds_R, rectified_image_target_bounds);

            rectified_image_bounds = object_2_image_transformation.Transform(object_coordinates);

            inverse_coeffs = ComputeDirectCoeffsFromPoints(rectified_image_bounds, image_coordinates);
        }  
        #endregion

        #region From a cross

        #region Unoptimized method based on a black cross on a white background
#pragma warning disable 1591
        /// <summary>
        /// Finds the ends of the model cross with a clic on its center (in pixel y-axis downside)
        /// </summary>
        private void GetCrossEnds()
        {
            Bitmap src = (Bitmap)source_image.Clone();
            BitmapData bmd_source_image = src.LockBits(new Rectangle(new Point(0, 0), source_image.Size), ImageLockMode.ReadWrite, source_image.PixelFormat);
            int PixelSize = 4;
            if (source_image.PixelFormat == System.Drawing.Imaging.PixelFormat.Format24bppRgb) { PixelSize = 3; }
            Point2D32S
                xmax_down = new Point2D32S(),
                xmax_up = new Point2D32S(),
                ymax_right = new Point2D32S(),
                ymax_left = new Point2D32S(),
                xmin_down = new Point2D32S(),
                xmin_up = new Point2D32S(),
            ymin_right = new Point2D32S(),
            ymin_left = new Point2D32S();
            Point2D64D[] cross_ends;

            xmin_down.X = source_image.Width; xmin_up.X = source_image.Width; ymin_right.X = source_image.Width; ymin_left.X = source_image.Width;
            xmin_down.Y = source_image.Height; xmin_up.Y = source_image.Height; ymin_right.Y = source_image.Height; ymin_left.Y = source_image.Height;

            linear_fill(clic_cross, ref clic_cross, ref xmax_down, ref xmax_up, ref ymax_right, ref ymax_left, ref xmin_down, ref xmin_up, ref ymin_right, ref ymin_left, ref bmd_source_image, PixelSize);
            if (xmax_up.Y + 1 == clic_cross.Y || xmax_down.Y - 1 == clic_cross.Y || ymax_right.X - 1 == clic_cross.X || ymax_left.X + 1 == clic_cross.X || xmin_up.Y + 1 == clic_cross.Y || xmin_down.Y - 1 == clic_cross.Y || ymin_right.X - 1 == clic_cross.X || ymin_left.X + 1 == clic_cross.X)
            {
                RectangleD cross_bounds = Tools.GetRectangleDBounds(new Point2D64D[] { xmax_down, xmax_up, ymax_right, ymax_left, xmin_down, xmin_up, ymin_right, ymin_left },YAxisDirection.DownSide);

                Point2D32S xmin, xmax, ymin, ymax;
                if (xmin_down.X == cross_bounds.X) { xmin = xmin_down; } else { xmin = xmin_up; }
                if (xmax_down.X == cross_bounds.X + cross_bounds.Width) { xmax = xmax_down; } else { xmax = xmax_up; }
                if (ymin_right.Y == cross_bounds.Y) { ymin = ymin_right; } else { ymin = ymin_left; }
                if (ymax_right.Y == cross_bounds.Y + cross_bounds.Height) { ymax = ymax_right; } else { ymax = ymax_left; }
                double angle = Math.Atan((xmax.Y - xmin.Y) / (xmax.X - xmin.X));
                cross_ends = new Point2D64D[]{
                    chercher_point_conjugué_g(xmin, (int)((cross_bounds.Width) / 5), angle,ref bmd_source_image, PixelSize),
                chercher_point_conjugué_h(ymin, (int)((cross_bounds.Height) / 5), angle,ref bmd_source_image, PixelSize),
                chercher_point_conjugué_d(xmax, (int)((cross_bounds.Width) / 5), angle,ref bmd_source_image, PixelSize),
                chercher_point_conjugué_b(ymax, (int)((cross_bounds.Height) / 5), angle,ref bmd_source_image, PixelSize)
                };
            }
            else
            {
                cross_ends = new Point2D64D[]{
                    chercher_conjugue(xmin_down, xmin_up, ymax_left),
                    chercher_conjugue(ymin_left, xmin_up, ymin_right),
                    chercher_conjugue(xmax_up, ymin_right, xmax_down),
                    chercher_conjugue(ymax_right, xmax_down, ymax_left)
                };
            }
            src.UnlockBits(bmd_source_image);
            image_coordinates = cross_ends;
        }

        private unsafe void linear_fill(Point2D32S Center, ref Point2D32S OriginalClic, ref Point2D32S xmax_down, ref Point2D32S xmax_up, ref Point2D32S ymax_right, ref Point2D32S ymax_left,
            ref Point2D32S xmin_down, ref Point2D32S xmin_up, ref Point2D32S ymin_right, ref Point2D32S ymin_left, ref BitmapData bmd_source_image, int pixel_size)
        {
            byte* source_image_ptr = (byte*)bmd_source_image.Scan0;
            int bord_g = Center.X;
            int bord_d = Center.X + 1;
            int pos_bord_g = (Center.Y * bmd_source_image.Stride / pixel_size + bord_g) * 3 + 2;
            int pos_bord_d = (Center.Y * bmd_source_image.Stride / pixel_size + bord_d) * 3 + 2;

            while (source_image_ptr[pos_bord_g] < Vars.Setup.StandardObjects.ImageRectification.CrossColorThreshold)
            {
                source_image_ptr[pos_bord_g] = 255;
                source_image_ptr[pos_bord_g - 1] = 0;
                source_image_ptr[pos_bord_g - 2] = 0;
                bord_g--;
                pos_bord_g -= 3;
            }
            bord_g++;
            pos_bord_g += 3;

            if (bord_g < xmin_up.X && Center.Y < OriginalClic.Y) { xmin_up.X = bord_g; xmin_up.Y = Center.Y; }
            if (bord_g < xmin_down.X && Center.Y > OriginalClic.Y) { xmin_down.X = bord_g; xmin_down.Y = Center.Y; }
            if (Center.Y < ymin_left.Y && bord_g < OriginalClic.X) { ymin_left.X = bord_g; ymin_left.Y = Center.Y; }
            if (Center.Y > ymax_left.Y && bord_g < OriginalClic.X) { ymax_left.X = bord_g; ymax_left.Y = Center.Y; }

            while (source_image_ptr[pos_bord_d] < Vars.Setup.StandardObjects.ImageRectification.CrossColorThreshold)
            {
                source_image_ptr[pos_bord_d] = 255;
                source_image_ptr[pos_bord_d - 1] = 0;
                source_image_ptr[pos_bord_d - 2] = 0;
                bord_d++;
                pos_bord_d += 3;
            }
            bord_d--;
            pos_bord_d -= 3;

            if (bord_d > xmax_up.X && Center.Y < OriginalClic.Y) { xmax_up.X = bord_d; xmax_up.Y = Center.Y; }
            if (bord_d > xmax_down.X && Center.Y > OriginalClic.Y) { xmax_down.X = bord_d; xmax_down.Y = Center.Y; }
            if (Center.Y < ymin_right.Y && bord_d > OriginalClic.X) { ymin_right.X = bord_d; ymin_right.Y = Center.Y; }
            if (Center.Y > ymax_right.Y && bord_d > OriginalClic.X) { ymax_right.X = bord_d; ymax_right.Y = Center.Y; }

            for (int i = bord_g; i <= bord_d; i++)
            {
                if (source_image_ptr[((Center.Y - 1) * bmd_source_image.Stride / pixel_size + i) * 3] < Vars.Setup.StandardObjects.ImageRectification.CrossColorThreshold) { linear_fill(new Point2D32S(i, Center.Y - 1), ref OriginalClic, ref xmax_down, ref xmax_up, ref ymax_right, ref ymax_left, ref xmin_down, ref xmin_up, ref ymin_right, ref ymin_left, ref bmd_source_image, pixel_size); }
                if (source_image_ptr[((Center.Y + 1) * bmd_source_image.Stride / pixel_size + i) * 3] < Vars.Setup.StandardObjects.ImageRectification.CrossColorThreshold) { linear_fill(new Point2D32S(i, Center.Y + 1), ref OriginalClic, ref xmax_down, ref xmax_up, ref ymax_right, ref ymax_left, ref xmin_down, ref xmin_up, ref ymin_right, ref ymin_left, ref bmd_source_image, pixel_size); }
            }
        }

        private unsafe Point2D64D chercher_point_conjugué_g(Point2D32S Centre, int Rayon, double Angle, ref BitmapData bmd_source_image, int pixel_size)
        {
            byte* source_image_ptr = (byte*)bmd_source_image.Scan0;

            Point2D64D moyenne = new Point2D64D();
            Point2D64D pt1, pt2;
            Point2D64D ref1, ref2;
            Point2D64D pos = new Point2D64D();
            ref1 = rotation_coordonnees(new Point2D64D(Centre.X - Rayon, Centre.Y + Rayon), Centre.X, Centre.Y, Angle);
            ref2 = rotation_coordonnees(new Point2D64D(Centre.X - Rayon, Centre.Y - Rayon), Centre.X, Centre.Y, Angle);
            pt1 = new Point2D64D();
            pt2 = new Point2D64D();
            double d1, d2, dt1, dt2;
            d1 = 2 * source_image.Width;
            d2 = 2 * source_image.Width;
            int bordx = Centre.X - Rayon / 2;
            if (bordx < 0) { bordx = 0; }
            for (int l = Centre.Y - Rayon; l < Centre.Y + Rayon; l++)
            {
                int pos_bord = (l * bmd_source_image.Stride / pixel_size + bordx) * 3 + 2;
                for (int c = bordx; c < Centre.X + Rayon / 2; c++)
                {
                    if (source_image_ptr[pos_bord] == 255 && source_image_ptr[pos_bord - 1] == 0 && source_image_ptr[pos_bord - 2] == 0)
                    {
                        pos.X = c; pos.Y = l;
                        dt1 = GeoMath.Distance2D(ref1, pos);
                        dt2 = GeoMath.Distance2D(ref2, pos);
                        if (dt1 < d1) { d1 = dt1; pt1.X = c; pt1.Y = l; }
                        if (dt2 < d2) { d2 = dt2; pt2.X = c; pt2.Y = l; }
                        break;
                    }
                    pos_bord += 3;
                }
            }
            moyenne = (pt1 + pt2) / 2.0;
            return moyenne;
        }

        private unsafe Point2D64D chercher_point_conjugué_h(Point2D32S Centre, int Rayon, double Angle, ref BitmapData bmd_source_image, int pixel_size)
        {
            byte* source_image_ptr = (byte*)bmd_source_image.Scan0;

            Point2D64D moyenne = new Point2D64D();
            Point2D64D pt1, pt2;
            Point2D64D ref1, ref2;
            Point2D64D pos = new Point2D64D();
            ref1 = rotation_coordonnees(new Point2D64D(Centre.X - Rayon, Centre.Y - Rayon), Centre.X, Centre.Y, Angle);
            ref2 = rotation_coordonnees(new Point2D64D(Centre.X + Rayon, Centre.Y - Rayon), Centre.X, Centre.Y, Angle);
            pt1 = new Point2D64D();
            pt2 = new Point2D64D();
            double d1, d2, dt1, dt2;
            d1 = 2 * source_image.Width;
            d2 = 2 * source_image.Width;
            int bordy = Centre.Y - Rayon / 2;
            if (bordy < 0) { bordy = 0; }
            for (int c = Centre.X - Rayon; c < Centre.X + Rayon; c++)
            {

                int pos_bord = (bordy * bmd_source_image.Stride / pixel_size + c) * 3 + 2;

                for (int l = bordy; l < Centre.Y + Rayon / 2; l++)
                {
                    if (source_image_ptr[pos_bord] == 255 && source_image_ptr[pos_bord - 1] == 0 && source_image_ptr[pos_bord - 2] == 0)
                    {
                        pos.X = c; pos.Y = l;
                        dt1 = GeoMath.Distance2D(ref1, pos);
                        dt2 = GeoMath.Distance2D(ref2, pos);
                        if (dt1 < d1) { d1 = dt1; pt1.X = c; pt1.Y = l; }
                        if (dt2 < d2) { d2 = dt2; pt2.X = c; pt2.Y = l; }
                        break;
                    }
                    pos_bord += 3 * bmd_source_image.Stride / pixel_size;
                }
            }
            moyenne = (pt1 + pt2) / 2.0;
            return moyenne;
        }

        private unsafe Point2D64D chercher_point_conjugué_d(Point2D32S Centre, int Rayon, double Angle, ref BitmapData bmd_source_image, int pixel_size)
        {
            byte* source_image_ptr = (byte*)bmd_source_image.Scan0;

            Point2D64D moyenne = new Point2D64D();
            Point2D64D pt1, pt2;
            Point2D64D ref1, ref2;
            Point2D64D pos = new Point2D64D();
            ref1 = rotation_coordonnees(new Point2D64D(Centre.X + Rayon, Centre.Y - Rayon), Centre.X, Centre.Y, Angle);
            ref2 = rotation_coordonnees(new Point2D64D(Centre.X + Rayon, Centre.Y + Rayon), Centre.X, Centre.Y, Angle);
            pt1 = new Point2D64D();
            pt2 = new Point2D64D();
            double d1, d2, dt1, dt2;
            d1 = 2 * source_image.Width;
            d2 = 2 * source_image.Width;
            int bordx = Centre.X + Rayon / 2;
            if (bordx >= source_image.Width) { bordx = source_image.Width - 1; }
            for (int l = Centre.Y - Rayon; l < Centre.Y + Rayon; l++)
            {

                int pos_bord = (l * bmd_source_image.Stride / pixel_size + bordx) * 3 + 2;
                for (int c = bordx; c > Centre.X - Rayon / 2; c--)
                {
                    if (source_image_ptr[pos_bord] == 255 && source_image_ptr[pos_bord - 1] == 0 && source_image_ptr[pos_bord - 2] == 0)
                    {
                        pos.X = c; pos.Y = l;
                        dt1 = GeoMath.Distance2D(ref1, pos);
                        dt2 = GeoMath.Distance2D(ref2, pos);
                        if (dt1 < d1) { d1 = dt1; pt1.X = c; pt1.Y = l; }
                        if (dt2 < d2) { d2 = dt2; pt2.X = c; pt2.Y = l; }
                        break;
                    }
                    pos_bord -= 3;
                }
            }
            moyenne = (pt1 + pt2) / 2.0;
            return moyenne;
        }

        private unsafe Point2D64D chercher_point_conjugué_b(Point2D32S Centre, int Rayon, double Angle, ref BitmapData bmd_source_image, int pixel_size)
        {
            byte* source_image_ptr = (byte*)bmd_source_image.Scan0;

            Point2D64D moyenne = new Point2D64D();
            Point2D64D pt1, pt2;
            Point2D64D ref1, ref2;
            Point2D64D pos = new Point2D64D();
            ref1 = rotation_coordonnees(new Point2D64D(Centre.X + Rayon, Centre.Y + Rayon), Centre.X, Centre.Y, Angle);
            ref2 = rotation_coordonnees(new Point2D64D(Centre.X - Rayon, Centre.Y + Rayon), Centre.X, Centre.Y, Angle);
            pt1 = new Point2D64D();
            pt2 = new Point2D64D();
            double d1, d2, dt1, dt2;
            d1 = 2 * source_image.Width;
            d2 = 2 * source_image.Width;
            int bordy = Centre.Y + Rayon / 2;
            if (bordy >= source_image.Height) { bordy = source_image.Height - 1; }
            for (int c = Centre.X - Rayon; c < Centre.X + Rayon; c++)
            {

                int pos_bord = (bordy * bmd_source_image.Stride / pixel_size + c) * 3 + 2;

                for (int l = bordy; l > Centre.Y - Rayon / 2; l--)
                {
                    if (source_image_ptr[pos_bord] == 255 && source_image_ptr[pos_bord - 1] == 0 && source_image_ptr[pos_bord - 2] == 0)
                    {
                        pos.X = c; pos.Y = l;
                        dt1 = GeoMath.Distance2D(ref1, pos);
                        dt2 = GeoMath.Distance2D(ref2, pos);
                        if (dt1 < d1) { d1 = dt1; pt1.X = c; pt1.Y = l; }
                        if (dt2 < d2) { d2 = dt2; pt2.X = c; pt2.Y = l; }
                        break;
                    }
                    pos_bord -= 3 * bmd_source_image.Stride / pixel_size;
                }
            }
            moyenne = (pt1 + pt2) / 2.0;
            return moyenne;
        }

        private Point2D64D rotation_coordonnees(Point2D64D Point, int Tx, int Ty, double Angle)
        {
            return new Point2D64D(
                (Point.X - Tx) * Math.Cos(Angle) - (Point.Y - Ty) * Math.Sin(Angle) + Tx,
                (Point.X - Tx) * Math.Sin(Angle) + (Point.Y - Ty) * Math.Cos(Angle) + Ty
            );
        }

        private Point2D64D chercher_conjugue(Point2D64D point, Point2D64D test1, Point2D64D test2)
        {
            Point2D64D point_moyenne = new Point2D64D();
            if (Math.Sqrt((point.X - test1.X) * (point.X - test1.X) + (point.Y - test1.Y) * (point.Y - test1.Y)) < Math.Sqrt((point.X - test2.X) * (point.X - test2.X) + (point.Y - test2.Y) * (point.Y - test2.Y)))
            {
                point_moyenne = (point + test1) / 2.0;
            }
            else
            {
                point_moyenne = (point + test2) / 2.0;
            }
            return point_moyenne;
        }

        //Generates 4 pairs of object coordinates clockwise, radius in mm
        private Point2D64D[] GenerateCrossObjectCoordinates(double Radius)
        {
            return new Point2D64D[]{
                new Point2D64D(0,Radius),
                new Point2D64D(Radius,2*Radius),
                new Point2D64D(2*Radius,Radius),
                new Point2D64D(Radius,0)
            };
        }
#pragma warning disable 1591
        #endregion

        #endregion

        #region From lines

        #region Unoptimized method but working
#pragma warning disable 1591
        //calcule les intersections des 4 droites 2H et 2V
        //d'abords les 2 lignes horizontales, puis les 2 verticales
        Point2D64D[] pt_intersection(Line2D64D[] LinesHV)
        {
            Point2D64D[] dst = new Point2D64D[4];

            dst[0] = LinesHV[0].LinesIntersection(LinesHV[2]);
            dst[1] = LinesHV[1].LinesIntersection(LinesHV[2]);
            dst[2] = LinesHV[1].LinesIntersection(LinesHV[3]);
            dst[3] = LinesHV[0].LinesIntersection(LinesHV[3]);

            return dst;
        }

        //point de fuit par le produit vectoriel
        Point3D64D point_de_fuite_cross(Line2D64D[] LinesHV)
        {
            Point2D64D VanishingPointH, VanishingPointV;

            VanishingPointH = LinesHV[0].LinesIntersection(LinesHV[1]);
            VanishingPointV = LinesHV[2].LinesIntersection(LinesHV[3]);

            Point3D64D VanishingLine = GeoMath.CrossProduct(VanishingPointH, VanishingPointV);
            VanishingLine.Homogenize();

            return VanishingLine;
        }

        //calcul de centre et rayon du cercle de contrainte avec angle droit
        Point2D64D contrainte_angle_droit(Point2D64D[] IntersectionPoints, Point3D64D VanishingLine)
        {
            Point2D64D c1 = new Point2D64D();
            Point3D64D pt1 = new Point3D64D();
            Point3D64D pt2 = new Point3D64D();
            Point3D64D pt3 = new Point3D64D();
            Point3D64D l1 = new Point3D64D();
            Point3D64D l2 = new Point3D64D();
            pt1.X = IntersectionPoints[0].X / (VanishingLine.X * IntersectionPoints[0].X + VanishingLine.Y * IntersectionPoints[0].Y + 1);
            pt1.Y = IntersectionPoints[0].Y / (VanishingLine.X * IntersectionPoints[0].X + VanishingLine.Y * IntersectionPoints[0].Y + 1);
            pt1.Z = 1;
            pt2.X = IntersectionPoints[1].X / (VanishingLine.X * IntersectionPoints[1].X + VanishingLine.Y * IntersectionPoints[1].Y + 1);
            pt2.Y = IntersectionPoints[1].Y / (VanishingLine.X * IntersectionPoints[1].X + VanishingLine.Y * IntersectionPoints[1].Y + 1);
            pt2.Z = 1;
            pt3.X = IntersectionPoints[3].X / (VanishingLine.X * IntersectionPoints[3].X + VanishingLine.Y * IntersectionPoints[3].Y + 1);
            pt3.Y = IntersectionPoints[3].Y / (VanishingLine.X * IntersectionPoints[3].X + VanishingLine.Y * IntersectionPoints[3].Y + 1);
            pt3.Z = 1;
            l1 = GeoMath.CrossProduct(pt1, pt2);
            l1.Homogenize();
            l2 = GeoMath.CrossProduct(pt1, pt3);
            l2.Homogenize();
            c1.X = ((-l1.Y / l1.X - l2.Y / l2.X) / 2);
            c1.Y = Math.Abs((-l1.Y / l1.X + l2.Y / l2.X) / 2);
            return c1;
        }

        //calcule le centre et rayon du cercle de contrainte avec ratio de longueur
        Point2D64D contrainte_ratio(Line2D64D[] LinesContraintes,double d1, double d2, Point3D64D VanishingLine)
        {
            Point2D64D c2 = new Point2D64D();
            Point3D64D pt1 = new Point3D64D();
            Point3D64D pt2 = new Point3D64D();
            Point3D64D pt3 = new Point3D64D();
            Point3D64D pt4 = new Point3D64D();
            pt1.X = LinesContraintes[0].Point1.X / (VanishingLine.X * LinesContraintes[0].Point1.X + VanishingLine.Y * LinesContraintes[0].Point1.Y + 1);
            pt1.Y = LinesContraintes[0].Point1.Y / (VanishingLine.X * LinesContraintes[0].Point1.X + VanishingLine.Y * LinesContraintes[0].Point1.Y + 1);
            pt1.Z = 1;
            pt2.X = LinesContraintes[0].Point2.X / (VanishingLine.X * LinesContraintes[0].Point2.X + VanishingLine.Y * LinesContraintes[0].Point2.Y + 1);
            pt2.Y = LinesContraintes[0].Point2.Y / (VanishingLine.X * LinesContraintes[0].Point2.X + VanishingLine.Y * LinesContraintes[0].Point2.Y + 1);
            pt2.Z = 1;
            pt3.X = LinesContraintes[1].Point1.X / (VanishingLine.X * LinesContraintes[1].Point1.X + VanishingLine.Y * LinesContraintes[1].Point1.Y + 1);
            pt3.Y = LinesContraintes[1].Point1.Y / (VanishingLine.X * LinesContraintes[1].Point1.X + VanishingLine.Y * LinesContraintes[1].Point1.Y + 1);
            pt3.Z = 1;
            pt4.X = LinesContraintes[1].Point2.X / (VanishingLine.X * LinesContraintes[1].Point2.X + VanishingLine.Y * LinesContraintes[1].Point2.Y + 1);
            pt4.Y = LinesContraintes[1].Point2.Y / (VanishingLine.X * LinesContraintes[1].Point2.X + VanishingLine.Y * LinesContraintes[1].Point2.Y + 1);
            pt4.Z = 1;
            c2.X = ((pt1.X - pt2.X) * (pt1.Y - pt2.Y) - (d1 / d2) * (d1 / d2) * (pt3.X - pt4.X) * (pt3.Y - pt4.Y)) / ((pt1.Y - pt2.Y) * (pt1.Y - pt2.Y) - (d1 / d2) * (d1 / d2) * (pt3.Y - pt4.Y) * (pt3.Y - pt4.Y));
            c2.Y = Math.Abs((d1 / d2) * ((pt3.X - pt4.X) * (pt1.Y - pt2.Y) - (pt1.X - pt2.X) * (pt3.Y - pt4.Y)) / ((pt1.Y - pt2.Y) * (pt1.Y - pt2.Y) - (d1 / d2) * (d1 / d2) * (pt3.Y - pt4.Y) * (pt3.Y - pt4.Y)));
            return c2;
        }

        //intersection de cercles si les centre sont sur le meme axe x
        Point2D64D intersection_cercle(Point2D64D ContrainteAngleDroit, Point2D64D ContrainteRatio)
        {
            double a, alpha;
            Point2D64D inter = new Point2D64D();
            a = Math.Abs(ContrainteRatio.X - ContrainteAngleDroit.X);
            alpha = Math.Acos((ContrainteRatio.Y * ContrainteRatio.Y - ContrainteAngleDroit.Y * ContrainteAngleDroit.Y - a * a) / (-2 * a * ContrainteAngleDroit.Y));
            if (ContrainteAngleDroit.X > ContrainteRatio.X) { alpha = Math.PI - alpha; };
            inter.X = ContrainteAngleDroit.X + ContrainteAngleDroit.Y * Math.Cos(alpha);
            inter.Y = ContrainteAngleDroit.Y * Math.Sin(alpha);
            return inter;
        }

        //calcul l'angle d'horizon
        double contrainte_angle_rotation(Point2D64D[] PointsIntersection, Point2D64D ContrainteCommune, Point3D64D VanishingLine)
        {
            double[] hz = new double[4];
            double angle;
            hz[0] = (PointsIntersection[0].X / ContrainteCommune.Y - PointsIntersection[0].Y * ContrainteCommune.X / ContrainteCommune.Y) / (PointsIntersection[0].X * VanishingLine.X + PointsIntersection[0].Y * VanishingLine.Y + 1);
            hz[1] = (PointsIntersection[0].Y) / (PointsIntersection[0].X * VanishingLine.X + PointsIntersection[0].Y * VanishingLine.Y + 1);
            hz[2] = (PointsIntersection[3].X / ContrainteCommune.Y - PointsIntersection[3].Y * ContrainteCommune.X / ContrainteCommune.Y) / (PointsIntersection[3].X * VanishingLine.X + PointsIntersection[3].Y * VanishingLine.Y + 1);
            hz[3] = (PointsIntersection[3].Y) / (PointsIntersection[3].X * VanishingLine.X + PointsIntersection[3].Y * VanishingLine.Y + 1);
            angle = -Math.Atan((hz[3] - hz[1]) / (hz[2] - hz[0]));
            return angle;
        }

        //calcule l'echelle globale
        double contrainte_echelle(Line2D64D[] LinesContraintes, Point2D64D ContrainteCommune, Point3D64D VanishingLine, double angle, double d1, double d2)
        {
            double echelle;
            Point2D64D coord_depart1 = new Point2D64D();
            Point2D64D coord_fin1 = new Point2D64D();
            Point2D64D coord_depart2 = new Point2D64D();
            Point2D64D coord_fin2 = new Point2D64D();
            coord_depart1.X = LinesContraintes[0].Point1.X;
            coord_depart1.Y = LinesContraintes[0].Point1.Y;
            coord_depart2.X = LinesContraintes[0].Point2.X;
            coord_depart2.Y = LinesContraintes[0].Point2.Y;
            coord_fin1 = contraintes_calcul_coordonnees(coord_depart1, ContrainteCommune, VanishingLine, angle);
            coord_fin2 = contraintes_calcul_coordonnees(coord_depart2, ContrainteCommune, VanishingLine, angle);
            echelle = Math.Sqrt((coord_fin1.X - coord_fin2.X) * (coord_fin1.X - coord_fin2.X) + (coord_fin1.Y - coord_fin2.Y) * (coord_fin1.Y - coord_fin2.Y)) / d1;
            coord_depart1.X = LinesContraintes[1].Point1.X;
            coord_depart1.Y = LinesContraintes[1].Point1.Y;
            coord_depart2.X = LinesContraintes[1].Point2.X;
            coord_depart2.Y = LinesContraintes[1].Point2.Y;
            coord_fin1 = contraintes_calcul_coordonnees(coord_depart1, ContrainteCommune, VanishingLine, angle);
            coord_fin2 = contraintes_calcul_coordonnees(coord_depart2, ContrainteCommune, VanishingLine, angle);
            echelle += Math.Sqrt((coord_fin1.X - coord_fin2.X) * (coord_fin1.X - coord_fin2.X) + (coord_fin1.Y - coord_fin2.Y) * (coord_fin1.Y - coord_fin2.Y)) / d2;
            echelle = echelle / 2;
            return echelle;
        }

        //Calcul les coordonnées finales avec les coeffs geometriques
        Point2D64D contraintes_calcul_coordonnees(Point2D64D coin, Point2D64D ContrainteCommune, Point3D64D VanishingLine, double angle)//calcul de coordonnées image vers monde réel
        {
            Point2D64D coins = new Point2D64D();
            coins.X = (coin.X * Math.Cos(angle) / ContrainteCommune.Y - coin.Y * (Math.Cos(angle) * ContrainteCommune.X / ContrainteCommune.Y + Math.Sin(angle))) / (coin.X * VanishingLine.X + coin.Y * VanishingLine.Y + 1);
            coins.Y = (coin.X * Math.Sin(angle) / ContrainteCommune.Y + coin.Y * (-Math.Sin(angle) * ContrainteCommune.X / ContrainteCommune.Y + Math.Cos(angle))) / (coin.X * VanishingLine.X + coin.Y * VanishingLine.Y + 1);
            return coins;
        }

        //Calcul les coordonnées originales avec les coeffs geometriques
        Point2D64D contraintes_calcul_coordonnees_inverse(Point2D64D coin, Point2D64D ContrainteCommune, Point3D64D VanishingLine, double angle)//calcul de coordonnées image vers monde réel
        {
            Point2D64D coordf = new Point2D64D();
            double bb, cc, dd, ee, r11, r12, r21, r22, r31, r32;
            bb = Math.Cos(angle) / ContrainteCommune.Y;
            cc = -ContrainteCommune.X * Math.Cos(angle) / ContrainteCommune.Y - Math.Sin(angle);
            dd = Math.Sin(angle) / ContrainteCommune.Y;
            ee = -ContrainteCommune.X * Math.Sin(angle) / ContrainteCommune.Y + Math.Cos(angle);
            r11 = ee / (bb * ee - cc * dd);
            r12 = -cc / (bb * ee - cc * dd);
            r21 = -dd / (bb * ee - cc * dd);
            r22 = cc * dd / ((bb * ee - cc * dd) * ee) + 1 / ee;
            r31 = (dd * VanishingLine.Y - ee * VanishingLine.X) / (bb * ee - cc * dd);
            r32 = -(bb * VanishingLine.Y - cc * VanishingLine.X) / (bb * ee - cc * dd);
            coordf.X = (r11 * coin.X + r12 * coin.Y) / (r31 * coin.X + r32 * coin.Y + 1);
            coordf.Y = (r21 * coin.X + r22 * coin.Y) / (r31 * coin.X + r32 * coin.Y + 1);
            return coordf;
        }

        void ComputeCoefficientsFromLines()
        {
            #region Computation with geometrical constraints
            Point2D64D[] IntersectionPoints = pt_intersection(this.HorizontalVerticalLines);
            Point3D64D VanishingLine = point_de_fuite_cross(this.HorizontalVerticalLines);
            Point2D64D c1 = contrainte_angle_droit(IntersectionPoints, VanishingLine);
            Point2D64D c2 = contrainte_ratio(this.ConstraintsLines, this.ConstraintLine1distance, this.ConstraintLine2Distance, VanishingLine);
            Point2D64D ContrainteCommune = intersection_cercle(c1, c2);
            double angle = contrainte_angle_rotation(IntersectionPoints, ContrainteCommune, VanishingLine);
            double echelle = contrainte_echelle(this.ConstraintsLines, ContrainteCommune, VanishingLine, angle, this.ConstraintLine1distance, this.ConstraintLine2Distance);
            #endregion

            #region Translate to points coordinates
            double[] croix = new double[8];
            Point2D64D centre_croix = new Point2D64D();
            double[] coord_coin = new double[2];//x,y des coordonnées d'un coin de l'image redressée
            double[] coord_coins = new double[8];//x,y des coordonnées des coins de l'image redresée
            Point2D64D coin = new Point2D64D();//x,y des coordonnées d'un coin de l'image originale

            for (int i = 0; i < 4; i++)
            {
                centre_croix.X = IntersectionPoints[i].X;
                centre_croix.Y = IntersectionPoints[i].Y;
                coin = contraintes_calcul_coordonnees(centre_croix, ContrainteCommune, VanishingLine, angle);
                coord_coins[2 * i] = coin.X / echelle;
                coord_coins[2 * i + 1] = coin.Y / echelle;
            }
            croix[2] = (coord_coins[2] + coord_coins[4]) * 0.5;
            croix[3] = (coord_coins[3] + coord_coins[5]) * 0.5;
            croix[6] = (coord_coins[0] + coord_coins[6]) * 0.5;
            croix[7] = (coord_coins[1] + coord_coins[7]) * 0.5;
            if (coord_coins[0] < coord_coins[6])
            {
                croix[0] = (coord_coins[0] + coord_coins[2]) * 0.5;
                croix[1] = (coord_coins[1] + coord_coins[3]) * 0.5;
                croix[4] = (coord_coins[4] + coord_coins[6]) * 0.5;
                croix[5] = (coord_coins[5] + coord_coins[7]) * 0.5;
            }
            if (coord_coins[0] > coord_coins[6])
            {
                croix[4] = (coord_coins[0] + coord_coins[2]) * 0.5;
                croix[5] = (coord_coins[1] + coord_coins[3]) * 0.5;
                croix[0] = (coord_coins[4] + coord_coins[6]) * 0.5;
                croix[1] = (coord_coins[5] + coord_coins[7]) * 0.5;
            }

            object_coordinates[0] = new Point2D64D(croix[0], croix[1]);
            object_coordinates[1]= new Point2D64D(croix[2],croix[3]);
            object_coordinates[2]= new Point2D64D( croix[4], croix[5]);
            object_coordinates[3]= new Point2D64D( croix[6],croix[7]);


            for (int i = 0; i < 4; i++)
            {
                centre_croix.X = croix[2 * i] * echelle;
                centre_croix.Y = croix[2 * i + 1] * echelle;
                coin = contraintes_calcul_coordonnees_inverse(centre_croix, ContrainteCommune, VanishingLine, angle);
                image_coordinates[i] = new Point2D64D(coin.X, coin.Y);
            }

            image_coordinates = image_coordinates.InverseY(source_image.Height);
            #endregion

            ComputeCoeffsFromPoints();
        }
#pragma warning restore 1591
        #endregion

        #endregion

        #region From known distances
    
        void SortInputLines(out Point2D64D[] ImagePoints,out Point2D64D[] ObjectPoints, out Line2D64D[] ObjectLines)
        {
            List<Point2D64D> ImagePointsList = new List<Point2D64D>();
            List<Point2D64D> ObjectPointsList = new List<Point2D64D>();
            ObjectLines = DistancesLines.DeepClone<Line2D64D[]>();

            Point2D64D TempPoint;

            foreach (Line2D64D l in this.DistancesLines)
            {
                TempPoint = ImagePointsList.Find(delegate(Point2D64D p) { return p == l.Point1; });
                if (TempPoint == null)
                {
                    ImagePointsList.Add(l.Point1);
                }

                TempPoint = ImagePointsList.Find(delegate(Point2D64D p) { return p == l.Point2; });
                if (TempPoint == null)
                {
                    ImagePointsList.Add(l.Point2);
                }
            }

            foreach (Line2D64D l in ObjectLines)
            {
                TempPoint = ObjectPointsList.Find(delegate(Point2D64D p) { return p == l.Point1; });
                if (TempPoint == null)
                {
                    ObjectPointsList.Add(l.Point1);
                }

                TempPoint = ObjectPointsList.Find(delegate(Point2D64D p) { return p == l.Point2; });
                if (TempPoint == null)
                {
                    ObjectPointsList.Add(l.Point2);
                }
            }

            ImagePoints = ImagePointsList.ToArray();
            ObjectPoints = ObjectPointsList.ToArray();
        }

        void ComputeInitialCoordinatesValues(ref Line2D64D[] ObjectLines, ref Point2D64D[] ObjectPoints)
        {
            double Scale = 0;

            foreach (Line2D64D l in ObjectLines)
            {
                Scale += (double)(l.Tag) / l.Length;
            }

            Scale /= ObjectLines.Length;

            foreach (Point2D64D p in ObjectPoints)
            {
                p.X *= Scale;
                p.Y *= Scale;
            }

            Line2D64D TempLine = new Line2D64D(ObjectPoints[0], ObjectPoints[1]);
            double Angle = -TempLine.Angle + Vars.Constants.PI / 2;
            Point2D64D TempPoint;

            foreach (Point2D64D p in ObjectPoints)
            {
                TempPoint = GeoMath.PointRotation(p, Angle, ObjectPoints[0]);
                p.X = TempPoint.X;
                p.Y = TempPoint.Y;
            }
        }

        void ProcessObjectCoordinatesAdjustment(Point2D64D[] ObjectPoints, Line2D64D[] ObjectLines)
        {
            //Point 1 XY fixed, point 2 X=point1.x fixed

            CvTermCriteria term = new CvTermCriteria(100, 0.0001);
            double mq0 = 2;
            double previous_mq0 = double.MaxValue;
            int iteration = 0;

            SetPositionsInMatrix(ObjectPoints);

            CvMat MatA = Cv.CreateMat(ObjectLines.Length, 2 * ObjectPoints.Length - 3, MatrixType.F64C1);
            CvMat MatX = Cv.CreateMat(MatA.Cols, 1, MatrixType.F64C1);
            CvMat MatB = Cv.CreateMat(MatA.Rows, 1, MatrixType.F64C1);
            CvMat MatV = Cv.CreateMat(MatA.Rows, 1, MatrixType.F64C1);

            while ((Math.Abs((previous_mq0 - mq0) / previous_mq0) > term.Epsilon || iteration < 2) && iteration < term.MaxIter)
            {
                FillMatricesAB(ObjectPoints, ObjectLines, MatA, MatB);
                ComputeMatX(MatA, MatX, MatB);
                ComputeMatV(MatA, MatB, MatX, MatV);
                AddDx(ObjectPoints, MatX);
                previous_mq0 = mq0;
                mq0 = ComputeMq0(MatV, MatA.Rows - MatA.Cols);
                iteration++;
            }
        }

        void SetPositionsInMatrix(Point2D64D[] ObjectPoints)
        {
            for (int i = 0; i < ObjectPoints.Length; i++)
            {
                ObjectPoints[i].Tag = i;
            }
        }

        void FillMatricesAB(Point2D64D[] ObjectPoints, Line2D64D[] ObjectLines, CvMat MatA, CvMat MatB)
        {
            Cv.Zero(MatA);
            
            Line2D64D CurrentLine;

            for (int i = 0; i < ObjectLines.Length; i++)
            {
                CurrentLine=ObjectLines[i];

                if ((int)(CurrentLine.Point1.Tag) > 1)
                {
                    Cv.mSet(MatA, i, 2 * ((int)(CurrentLine.Point1.Tag) - 1) - 1, -DerivateDistanceDX2(CurrentLine.Point1, CurrentLine.Point2));
                    Cv.mSet(MatA, i, 2 * ((int)(CurrentLine.Point1.Tag) - 1), -DerivateDistanceDY2(CurrentLine.Point1, CurrentLine.Point2));
                    Cv.mSet(MatB, i, 0, (double)CurrentLine.Tag - CurrentLine.Length);
                }
                if ((int)(CurrentLine.Point1.Tag) == 1)
                {
                    Cv.mSet(MatA, i, 0, -DerivateDistanceDY2(CurrentLine.Point1, CurrentLine.Point2));
                    Cv.mSet(MatB, i, 0, (double)CurrentLine.Tag - CurrentLine.Length);
                }

                if ((int)(CurrentLine.Point2.Tag) > 1)
                {
                    Cv.mSet(MatA, i, 2 * ((int)(CurrentLine.Point2.Tag) - 1) - 1, DerivateDistanceDX2(CurrentLine.Point1, CurrentLine.Point2));
                    Cv.mSet(MatA, i, 2 * ((int)(CurrentLine.Point2.Tag) - 1), DerivateDistanceDY2(CurrentLine.Point1, CurrentLine.Point2));
                    Cv.mSet(MatB, i, 0, (double)CurrentLine.Tag - CurrentLine.Length);
                }
                if ((int)(CurrentLine.Point2.Tag) == 1)
                {
                    Cv.mSet(MatA, i, 0, DerivateDistanceDY2(CurrentLine.Point1, CurrentLine.Point2));
                    Cv.mSet(MatB, i, 0, (double)CurrentLine.Tag - CurrentLine.Length);
                }
            }
        }

        void ComputeMatX(CvMat MatA, CvMat MatX, CvMat MatB)
        {
            Cv.Solve(MatA, MatB, MatX, InvertMethod.Svd);
            /*CvMat N = Cv.CreateMat(MatA.Cols, MatA.Cols, MatrixType.F64C1);
            CvMat Nk = Cv.CreateMat(MatA.Cols, 1, MatrixType.F64C1);
            CvMat At=Cv.CreateMat(MatA.Cols,MatA.Rows,MatA.ElemType);

            Cv.MulTransposed(MatA, N, true);
            Cv.Transpose(MatA, At);
            Cv.MatMul(At, MatB, Nk);
            Cv.Invert(N, N, InvertMethod.SvdSymmetric);
            Cv.MatMul(N, Nk, MatX);*/
        }

        void ComputeMatV(CvMat MatA, CvMat MatB, CvMat MatX, CvMat MatV)
        {
            MatB.Scale(MatB, -1d);
            Cv.MatMulAdd(MatA, MatX, MatB, MatV);
        }

        double ComputeMq0(CvMat MatV, int nq)
        {
            CvMat mq0 = Cv.CreateMat(1, 1, MatrixType.F64C1);
            Cv.MulTransposed(MatV, mq0, true);
            return Math.Sqrt(mq0.mGet(0, 0) / nq);
        }

        void AddDx(Point2D64D[] ObjectPoints, CvMat Matx)
        {
            ObjectPoints[1].Y += Cv.mGet(Matx, 0, 0);

            for (int i = 2; i < ObjectPoints.Length; i++)
            {
                ObjectPoints[i].X += Cv.mGet(Matx, 2 * (i - 1) - 1, 0);
                ObjectPoints[i].Y += Cv.mGet(Matx, 2 * (i - 1), 0);
            }
        }

        double DerivateDistanceDX2(Point2D64D Point1, Point2D64D Point2)
        {
            //Distance = Sqrt((X2-X1)^2+(Y2-Y1)^2)

            return (Point2.X - Point1.X) / Point1.Distance2D(Point2);
        }

        double DerivateDistanceDY2(Point2D64D Point1, Point2D64D Point2)
        {
            //Distance = Sqrt((X2-X1)^2+(Y2-Y1)^2)

            return (Point2.Y - Point1.Y) / Point1.Distance2D(Point2);
        }

        void ComputeCoefficientsFromDistances()
        {
            Point2D64D[] ObjectPoints;
            Point2D64D[] ImagePoints;
            Line2D64D[] ObjectLines;
            

            SortInputLines(out ImagePoints, out ObjectPoints, out ObjectLines);
            ComputeInitialCoordinatesValues(ref ObjectLines, ref ObjectPoints);
            ProcessObjectCoordinatesAdjustment(ObjectPoints, ObjectLines);

            this.image_coordinates = ImagePoints.InverseY(this.source_image.Height);
            this.object_coordinates = ObjectPoints;
            ComputeCoeffsFromPoints();

            if (this.HorizontalVerticalLine != null)
            {
                Line2D64D ObjectHorizontalVerticalLine = this.HorizontalVerticalLine.DeepClone<Line2D64D>();
                ObjectHorizontalVerticalLine.Point1 = ComputeObjectCoordinates(ObjectHorizontalVerticalLine.Point1.InverseY(this.source_image.Height));
                ObjectHorizontalVerticalLine.Point2 = ComputeObjectCoordinates(ObjectHorizontalVerticalLine.Point2.InverseY(this.source_image.Height));

                double Angle = -ObjectHorizontalVerticalLine.Angle;

                if (!(bool)(HorizontalVerticalLine.Tag))
                {
                    Angle += Vars.Constants.PI / 2;
                }

                Point2D64D TempPoint;
                foreach (Point2D64D p in this.object_coordinates)
                {
                    TempPoint = GeoMath.PointRotation(p, Angle, ObjectHorizontalVerticalLine.Point1);
                    p.X = TempPoint.X;
                    p.Y = TempPoint.Y;
                }

                RectangleD ObjectPointsBounds = this.object_coordinates.GetRectangleDBounds(YAxisDirection.UpSide);

                foreach (Point2D64D p in this.object_coordinates)
                {
                    p.X -= ObjectPointsBounds.LowerLeftPoint.X;
                    p.Y -= ObjectPointsBounds.LowerLeftPoint.Y;
                }
                ComputeCoeffsFromPoints();
            }
        }
    
        #endregion

        #region Public method
        /// <summary>
        /// Computes the transformation coefficients whatever the technique is
        /// </summary>
        public void ComputeTransformationCoefficients()
        {
            try
            {
                if (rectification_technique == RectificationTechnique.Points)
                {
                    if (object_coordinates != null && image_coordinates != null)
                    {
                        ComputeCoeffsFromPoints();
                    }
                    else
                    {
                        throw new Exception("No images or object coordinates");
                    }
                }
                else if (rectification_technique == RectificationTechnique.Cross)
                {
                    object_coordinates = GenerateCrossObjectCoordinates(cross_radius);
                    if (image_coordinates == null)
                    {
                        GetCrossEnds();
                    }
                    if (object_coordinates != null)
                    {
                        ComputeCoeffsFromPoints();
                    }
                    else
                    {
                        throw new Exception("No point clicked");
                    }
                }
                else if (rectification_technique == RectificationTechnique.Lines)
                {
                    ComputeCoefficientsFromLines();
                }
                else if (rectification_technique == RectificationTechnique.Distances)
                {
                    ComputeCoefficientsFromDistances();
                }
            }
            catch
            {
                MessageBox.Show("Check lines/points", "Error in computing transformation", MessageBoxButtons.OK, MessageBoxIcon.Error);
            }
        } 
        #endregion
        #endregion

        #region Rectified image generation
        /// <summary>
        ///Generates the rectified image from the original image
        ///The principle is to scan an empty rectified image, and for each pixel of this image
        ///we compute the coordinates of the analogous pixel in the original image, so there's no hole in
        ///the final image
        /// </summary>
        public unsafe Bitmap GenerateRectifiedImage()
        {
            Bitmap rectified_image = new Bitmap(rectified_image_size.Width, rectified_image_size.Height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
            BitmapData bmdoutput = rectified_image.LockBits(new Rectangle(new Point(0, 0), rectified_image_size), ImageLockMode.WriteOnly, rectified_image.PixelFormat);
            BitmapData bmdsource = source_image.LockBits(new Rectangle(new Point(0, 0), source_image.Size), ImageLockMode.ReadOnly, source_image.PixelFormat);
            int PixelSize = 4;
            if (source_image.PixelFormat == System.Drawing.Imaging.PixelFormat.Format24bppRgb) { PixelSize = 3; }
            byte* ptrin = (byte*)bmdsource.Scan0;
            byte* ptrout = (byte*)bmdoutput.Scan0;
            int posout, pos, xin, yin, h = rectified_image_size.Height, w = rectified_image_size.Width, hin = source_image.Height, win = source_image.Width, bmdsourceStride = bmdsource.Stride;
            double numx0, numy0, denom0;
            double numx = inverse_coeffs.a1, numy = inverse_coeffs.b1, denom = inverse_coeffs.c1;
            for (int y = 0; y < h; y++)
            {
                posout = (rectified_image_size.Height - y - 1) * bmdoutput.Stride;
                numx0 = inverse_coeffs.a2 * y + inverse_coeffs.a3;
                numy0 = inverse_coeffs.b2 * y + inverse_coeffs.b3;
                denom0 = inverse_coeffs.c2 * y + 1;

                for (int x = 0; x < w; x++)
                {
                    xin = (int)(numx0 / denom0);
                    yin = (int)(numy0 / denom0);
                    pos = yin * bmdsourceStride + xin * PixelSize;
                    if (xin >= 0 && xin < win && yin >= 0 && yin < hin)
                    {
                        ptrout[posout] = ptrin[pos];
                        ptrout[posout + 1] = ptrin[pos + 1];
                        ptrout[posout + 2] = ptrin[pos + 2];
                    }
                    posout += 3;
                    numx0 += numx;
                    numy0 += numy;
                    denom0 += denom;
                }
            }
            source_image.UnlockBits(bmdsource);
            rectified_image.UnlockBits(bmdoutput);
            return rectified_image;
        }

        /// <summary>
        /// Gets the rectified image
        /// </summary>
        public GeoTaggedImage GetRectifiedImageGeoImage()
        {
            GeoTaggedImage dst = new GeoTaggedImage(GenerateRectifiedImage(),new Layer());
            dst.ImageExtents = object_2_image_transformation.InverseTransform(dst.ImageExtents,YAxisDirection.UpSide);
            return dst;
        }
        #endregion

        #region Coordinates computation
        /// <summary>
        /// Computes the object coordinate of a pixel from the original
        /// </summary>
        /// <param name="Point">The point from the image</param>
        public Point2D64D ComputeObjectCoordinates(Point2D64D Point)
        {
            return new Point2D64D(
            (direct_coeffs.a1 * Point.X + direct_coeffs.a2 * Point.Y + direct_coeffs.a3) / (direct_coeffs.c1 * Point.X + direct_coeffs.c2 * Point.Y + 1),
            (direct_coeffs.b1 * Point.X + direct_coeffs.b2 * Point.Y + direct_coeffs.b3) / (direct_coeffs.c1 * Point.X + direct_coeffs.c2 * Point.Y + 1)
            );
        }

        /// <summary>
        /// Computes the original coordinate of a pixel from the object
        /// </summary>
        /// <param name="Point">The point from the object</param>
        public Point2D64D ComputeImageCoordinates(Point2D64D Point)
        {
            return new Point2D64D(
            (inverse_coeffs.a1 * Point.X + inverse_coeffs.a2 * Point.Y + direct_coeffs.a3) / (inverse_coeffs.c1 * Point.X + inverse_coeffs.c2 * Point.Y + 1),
            (inverse_coeffs.b1 * Point.X + inverse_coeffs.b2 * Point.Y + inverse_coeffs.b3) / (inverse_coeffs.c1 * Point.X + inverse_coeffs.c2 * Point.Y + 1)
            );
        }  
        #endregion

        #region Misc
        private void Reset()
        {
            direct_coeffs = null;
            inverse_coeffs = null;
            object_2_image_transformation = null;
        } 
        #endregion
        #endregion

        #region Properties
        private int PointsCount
        {
            get
            {
                return image_coordinates.Length;
            }
        }

        /// <summary>
        /// Gets or sets the image coordinates for a rectification from known points
        /// </summary>
        public Point2D64D[] ImageCoordinates
        {
            get
            {
                return image_coordinates;
            }
            set
            {
                image_coordinates = value;
                Reset();
            }
        }

        /// <summary>
        /// Gets or sets the object coordinates for a rectification from known points
        /// </summary>
        public Point2D64D[] ObjectCoordinates
        {
            get
            {
                return object_coordinates;
            }
            set
            {
                object_coordinates = value;
                Reset();
            }
        }

        /// <summary>
        /// Sets clic point on the cross for the cross based rectification technique
        /// </summary>
        public Point2D32S CrossClic
        {
            set
            {
                Reset();
                clic_cross = value;
            }
        }

        /// <summary>
        /// Gets or sets maximum size of the rectified image (pixels)
        /// </summary>
        public int RectifiedImageMaximumSize
        {
            get
            {
                return MaxSize;
            }
            set
            {
                Reset();
                MaxSize = value;
            }
        }
        #endregion
    }
}
